#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<geometry_msgs/PoseStamped.h>
bool mode=0;//0 for local_planner;1 for pid_planner;
ros::Publisher cmdvelPub;
void cmd_velCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    if (msg->linear.z==1)
    {
        if (mode) ROS_INFO("pid_planner success!");
        else ROS_INFO("local_planner success");
        mode=!mode;
    }   
}
void calib_velCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    if (mode==1)
    {
        cmdvelPub.publish(msg);
    }
}
int main(int argc,char **argv)
{
    ros::init(argc,argv, "mode_swithcher");
    ros::NodeHandle nh;
    ros::Subscriber itemPoseSub=nh.subscribe("/calib_vel",1,calib_velCallback);
    ros::Subscriber cmdvelSub=nh.subscribe("/cmd_vel",1,cmd_velCallback);
    cmdvelPub=nh.advertise<geometry_msgs::Twist>("/cmd_vel",1);
    ros::spin();
    return 0;   
}